#!/usr/bin/env python2
# -*- coding: utf-8 -*-
''' 测试用，处理测试用动捕数据，存成笔画 '''
import copy
import json
import pandas as pd
import numpy as np
from math import pi
from elfin_driver import TransMartrix2xyzrpy, xyzrpy2TransMartrix

# x,y,z,r,p,y [mm]
TARGET_TO_PEN = [0, 0, 0, pi, 0, 0]
INDEX = ''
DATA_PATH = INDEX + '.csv'
STROKE_PATH = './calligraphy_lib/strokes/record_test'+INDEX+'.json'
org_z = None  # 第一点Z记录为零点

def fileRead():
    '''读入文件，转化为xyzrpy列表并返回'''
    pd_data = pd.read_csv(DATA_PATH)  # 读取存放的文件
    datas = pd_data.values  # 转化为np
    datas_less = []
    for index,data in enumerate(datas):
        if index%20 == 0:
            datas_less.append(data.tolist())
    datas_less = np.array(datas_less)
    datas_less *= [1.0, 1.0, 1.0, pi/180.0, pi/180.0, pi/180.0]
    return datas_less

def transform(datas):
    global org_z
    '''原始靶标到笔尖的坐标变换'''
    datas_trans = []
    for data in datas:
        T_base2target = xyzrpy2TransMartrix(data)
        T_target2pen = xyzrpy2TransMartrix(TARGET_TO_PEN)
        T_base2pen = T_base2target.dot(T_target2pen)
        datas_trans.append(TransMartrix2xyzrpy(T_base2pen))  # 转换为基坐标系下笔尖的坐标列表
    org_z = datas_trans[0][2]
    return datas_trans

def extractAndSave(org_datas):
    '''提取轨迹点外接矩形，存为笔画格式'''
    stroke_points = copy.deepcopy(org_datas)
    x_min, y_min, x_max, y_max = float('inf'), float('inf'), float('-inf'), float('-inf')
    for org_data in org_datas:
        x_min = min(x_min, org_data[0])
        y_min = min(y_min, org_data[1])
        x_max = max(x_max, org_data[0])
        y_max = max(y_max, org_data[1])
    for i in range(len(stroke_points)):
        stroke_points[i] = list(stroke_points[i])
        stroke_points[i][0] -= x_min
        stroke_points[i][1] = y_max - stroke_points[i][1]
        stroke_points[i][2] -= org_z
    stroke_size = [x_max-x_min, y_max-y_min]
    stroke_obj = {
        'stroke_size':stroke_size,
        'stroke_name': 'record_test'+INDEX,
        'preview_img': 'record_test'+INDEX+'.png',
        'stroke_points': stroke_points
    }
    with open(STROKE_PATH, 'w') as f:
        f.write(json.dumps(stroke_obj, indent=4, ensure_ascii=False))


if __name__ == '__main__':
    datas = fileRead()
    datas_trans = transform(datas)
    extractAndSave(datas_trans)